#include "qmc5883l.h"

#include <usdev/i2c.h>
#include <usdev/i2c-dev.h>

qmc5883l::qmc5883l()
{

}

qmc5883l::~qmc5883l()
{

}

bool qmc5883l::init()
{
    int ret;

    ret = writeReg(0x0b, 0x01);
    ret |= writeReg(0x20, 0x40);
    ret |= writeReg(0x21, 0x01);
    ret |= writeReg(0x09, 0x0d);

    return (ret == 0);
}

bool qmc5883l::readMag(short &x, short &y, short &z)
{
    unsigned char buf[6];

    if (readRegs(0, buf, 6) != 0)
    {
        return false;
    }

    x = (int16_t)(buf[1] << 8 | buf[0]);
    y = (int16_t)(buf[3]<< 8 | buf[2]);
    z = (int16_t)(buf[5]<< 8 | buf[4]);

    return true;
}

int qmc5883l::writeReg(unsigned char reg, unsigned char val)
{
    int ret;
    struct rt_i2c_msg msg[1];
    struct i2c_rdwr_ioctl_data rw;
    uint8_t buf[2];

    buf[0] = reg;
    buf[1] = val;

    msg[0].addr = 0x0d;
    msg[0].flags = RT_I2C_WR;
    msg[0].buf = buf;
    msg[0].len = 2;

    rw.msgs = msg;
    rw.nmsgs = 1;

    ret = ioctl(I2C_RDWR, &rw);

    return ret;
}

int qmc5883l::readRegs(unsigned char reg, unsigned char *val, int size)
{
    int ret;
    struct rt_i2c_msg msg[2];
    struct i2c_rdwr_ioctl_data rw;

    msg[0].addr = 0x0d;
    msg[0].flags = RT_I2C_WR;
    msg[0].buf = &reg;
    msg[0].len = 1;

    msg[1].addr = 0x0d;
    msg[1].flags = RT_I2C_RD;
    msg[1].buf = val;
    msg[1].len = (uint16_t)size;

    rw.msgs = msg;
    rw.nmsgs = 2;

    ret = ioctl(I2C_RDWR, &rw);

    return ret;
}
